import os
from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

from launch_ros.actions import Node, ComposableNodeContainer
from launch_ros.descriptions import ComposableNode


def generate_launch_description():
    declare_use_sim_time = DeclareLaunchArgument('use_sim_time', default_value='true')
    declare_points_topic = DeclareLaunchArgument('points_topic', default_value='/rslidar_points_synced')
    declare_imu_topic    = DeclareLaunchArgument('imu_topic',    default_value='/dog_imu_raw_synced')
    declare_globalmap_pcd = DeclareLaunchArgument(
        'globalmap_pcd',
        default_value='src/hdl_Localization_ROS2/hdl_localization/data/DAM-563-Original-merge-lite-5cm-ascii.pcd'
    )
    default_rviz_config = os.path.join(
        get_package_share_directory('hdl_localization'), 'rviz', 'hdl_localization_ros2.rviz'
    )
    declare_rviz_config = DeclareLaunchArgument(
        'rviz_config', default_value=default_rviz_config
    )

    container = ComposableNodeContainer(
        name='hdl_localization_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=[
            ComposableNode(
                package='hdl_localization',
                plugin='hdl_localization::GlobalmapServerNodelet',
                name='GlobalmapServerNodelet',
                parameters=[
                    {'use_sim_time': LaunchConfiguration('use_sim_time')},
                    {'globalmap_pcd': LaunchConfiguration('globalmap_pcd')},
                    {'downsample_resolution': 0.1},
                    {'convert_utm_to_local': True},
                ],
            ),
            ComposableNode(
                package='hdl_localization',
                plugin='hdl_localization::HdlLocalizationNodelet',
                name='HdlLocalizationNodelet',
                remappings=[
                    ('/ouster/points', LaunchConfiguration('points_topic')),
                    ('/gpsimu_driver/imu_data', LaunchConfiguration('imu_topic')),
                ],
                parameters=[
                    {'use_sim_time': LaunchConfiguration('use_sim_time')},
                    {'robot_odom_frame_id': 'odom'},
                    {'odom_child_frame_id': 'base_link'},
                    {'enable_robot_odometry_prediction': False}, 
                    {'use_imu': True},
                    {'ndt_resolution': 0.5},
                    {'downsample_resolution': 0.2},
                    {'use_global_localization': False},
                    {'specify_init_pose': False},
                    {'ndt_maximum_iterations': 50},
                    {'map_intensity_threshold': 23.0}, 
                    {'ndt_neighbor_search_method': 'KDTREE'},
                    {'ndt_neighbor_search_radius': 4.0},
                    {'cool_time_duration': 1.0},
                ],
            ),
        ],
        output='screen',
        emulate_tty=True,
    )

    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        arguments=['-d', LaunchConfiguration('rviz_config')],
        parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}],
        output='screen',
        emulate_tty=True,
    )


    return LaunchDescription([
        declare_use_sim_time,
        declare_points_topic,
        declare_imu_topic,
        declare_globalmap_pcd,
        declare_rviz_config,
        container,
        rviz_node,
    ])